#include "chassis.h"

#define D 0.08548 // distance from the center of the platfrom to the center of the wheel
#define R 0.02895 // radius of the wheel

const float cos30 = 0.86602540378;

static XC330_Instance_s *motor_fr, *motor_l, *motor_r;
cmd_recv_s cmd_recv;
static Protocol_Config_s *protocol_instance;
static float uf, ur, ul;                         // velocity of the front, right, and left wheels (r/s)
static float target_vx_, target_vy_, target_wz_; // target velocity and angular velocity of the end-effector in world frame
void chassis_init()
{
    XC330_config_s motor_fr_config = {
        .id = 0x03,
        .drive_mode = Normal_Mode | Velocity_based_Profile | Torque_On,
        .operating_mode = Velocity_Control_Mode,
        // .velocity_Kp = 0.1,
        // .velocity_Ki = 0.1,
        // .velocity_limit = 1000,
    };
    motor_fr = XC330_Register(&motor_fr_config);
    DWT_Delay_ms(500);
    XC330_config_s motor_l_config = {
        .id = 0x02,
        .drive_mode = Normal_Mode | Velocity_based_Profile | Torque_On,
        .operating_mode = Velocity_Control_Mode,
        // .velocity_Kp = 0.1,
        // .velocity_Ki = 0.1,
        // .velocity_limit = 1000,
    };
    motor_l = XC330_Register(&motor_l_config);
    DWT_Delay_ms(500);
    XC330_config_s motor_r_config = {
        .id = 0x01,
        .drive_mode = Normal_Mode | Velocity_based_Profile | Torque_On,
        .operating_mode = Velocity_Control_Mode,
        // .velocity_Kp = 0.1,
        // .velocity_Ki = 0.1,
        // .velocity_limit = 1000,
    };
    motor_r = XC330_Register(&motor_r_config);
    DWT_Delay_ms(500);

    protocol_instance = ProtocolInit(&huart3);
    target_vx_ = 20;
    target_vy_ = 10;
    target_wz_ = 10;
}

// void forward_kinematics(float uf, float ur, float ul)
// {

// }

void chassis_cmd_recv_decode(void)
{
    cmd_recv.target_vbx_ = (float)(protocol_instance->data[0] << 8 | protocol_instance->data[1]) / 1000.0f;
    cmd_recv.target_vby_ = (float)(protocol_instance->data[2] << 8 | protocol_instance->data[3]) / 1000.0f;
    cmd_recv.target_wbz_ = (float)(protocol_instance->data[4] << 8 | protocol_instance->data[5]) / 1000.0f;
}

void chassis_cmd_send_feedback(void)
{
    uint8_t send_buf[11];
    send_buf[0] = (uint8_t)(target_vx_ * 1000) >> 8;
    send_buf[1] = (uint8_t)(target_vx_ * 1000);
    send_buf[2] = (uint8_t)(target_vy_ * 1000) >> 8;
    send_buf[3] = (uint8_t)(target_vy_ * 1000);
    send_buf[4] = (uint8_t)(target_wz_ * 1000) >> 8;
    send_buf[5] = (uint8_t)(target_wz_ * 1000);
    Protocol_Send(send_buf, 6);
}

void inverse_kinematics(float vx, float vy, float wz)
{
    uf = (1 / R) * (-D * wz - vx);
    ur = (1 / R) * (-D * wz - (1 / 2) * vx - cos30 * vy);
    ul = (1 / R) * (-D * wz - (1 / 2) * vx + cos30 * vy);
}

void chassis_task(void)
{
    chassis_cmd_recv_decode();
    inverse_kinematics(cmd_recv.target_vbx_, cmd_recv.target_vby_, cmd_recv.target_wbz_);
    // inverse_kinematics(1, 1, 1);
    XC330_SetVelocity(motor_fr, (int32_t)(uf));
    // DWT_Delay_ms(500);
    XC330_SetVelocity(motor_l, (int32_t)(ul));
    // DWT_Delay_ms(500);
    XC330_SetVelocity(motor_r, (int32_t)(ur));
    // DWT_Delay_ms(500);
}